import time
import cv2

import numpy as np
import random



def get_approx_list(approxs, hchy):
    '''
    返回多个连通区域
    一般只有一个，有多个细小的调 ratio
    '''
    returnlist = []
    for i in range(0, len(hchy)):
        hchylayer = 0
        hchyp = i
        while hchy[hchyp][3] != -1:# 有下一级轮廓
            hchyp = hchy[hchyp][3]# 下一级标号
            hchylayer += 1# 算层数
        #print(hchylayer)
        if (hchylayer % 2 == 0):
            temp_list = approxs[i].tolist()# 打包成一个
            hchyp = hchy[i][2] # 下一级
            if hchyp != -1:# 如果有下一级（一定是奇数级），不是最基础的轮廓,层数不为0
                temp_list.append([-1, -1])
                temp_list += approxs[hchyp].tolist()#这里加奇数级
                while hchy[hchyp][0] != -1:
                    hchyp = hchy[hchyp][0]
                    temp_list.append([-1, -1])
                    temp_list += approxs[hchyp].tolist()
            returnlist.append(temp_list)#层数是0直接添加


    # temp = np.ones((1463, 1218,3), np.uint8) * 255
    # cv2.drawContours(temp, approxs, -1, (0, 255, 0), 1)
    # for i in range(0, len(returnlist)):
    #     color = (random.randint(0, 255), random.randint(0, 255), random.randint(0, 255))
    #     #print(returnlist[i])
    #     for point in returnlist[i] :
    #          print(point)
    #          if point[0]!=-1:
    #             cv2.circle(temp, point, 5, color, 2)
    #          else:
    #             color = (random.randint(0, 255), random.randint(0, 255), random.randint(0, 255))
    # cv2.imshow('approxs', temp)
    # cv2.waitKey()


    return returnlist  # 返回拆分后连通性列表


def get_approx(img, robotpixel):
    # 提取轮廓
    contour, hierarchy = cv2.findContours(img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    # 提取轮廓
    '''
    findContours函数接收三个参数，分别是图像，轮廓检索模式，以及轮廓近似方法。
    
    opencv2返回两个值：一个是轮廓本身contours，还有一个是每条轮廓对应的属性hierarchy。
    contours：检测到的轮廓，以numpy数组的形式返回。
    hierarchy：轮廓的层次结构，以numpy数组的形式返回。
    offset：可选参数，表示轮廓中每个点的偏移量，通常设置为(0,0)。
    '''
    # print(len(contour))
    # print(hierarchy)
    approxs = [[]] * len(contour)
    hchy = [[]] * len(contour)
    epsilon = int(robotpixel / 3)
    # print(hierarchy)
    #print(img.shape)
    for i in range(0, len(contour)):
        approx = cv2.approxPolyDP(contour[i], epsilon, True)  # 将每个轮廓近似为一个多边形
        approxs[i] = approx[:, 0, :][::-1]
        hchy[i] = hierarchy[0][i]
        #print(hchy[i])

    # temp = np.ones(img.shape, np.uint8) * 255
    # # temp2 = np.ones(img.shape, np.uint8) * 255
    # # 画轮廓：temp为白色幕布，contours为轮廓， -1表示画所有轮廓，颜色：绿色，厚度
    # cv2.drawContours(temp, approxs, -1, (0, 255, 0), 3)
    # '''
    # 把第三个参数改一下可以看approxs里的每一个轮廓
    # image：需要绘制轮廓的图像，可以是原始图像或者是黑白图像。
    # contours：要绘制的轮廓。需要传入一个列表，列表中的每一个元素是一个轮廓，每个轮廓由若干个点坐标组成。
    # contourIdx：需要绘制的轮廓在列表中的索引，如果传入 -1，表示绘制所有的轮廓。
    # color：轮廓的颜色。可以是一个颜色值，如 (0, 255, 0)，表示绿色；也可以是一个 BGR 三通道的数组，如 [255, 0, 0]，表示蓝色。
    # thickness：轮廓线的粗细。默认值为 1，如果需要填充轮廓，则可以设置为 -1。
    # '''
    # #cv2.drawContours(temp2, contour,-1, (0, 255, 0), 3)
    # cv2.imshow('approxs', temp)
    # #cv2.imshow('contour', temp2)
    # cv2.waitKey()

    return get_approx_list(approxs, hchy)


def approx_work(IMGmap, ratio, robotsize, thresh):
    # 路径二值化
    img_gray = cv2.cvtColor(IMGmap, cv2.COLOR_BGR2GRAY)
    ret, img_bin = cv2.threshold(img_gray, thresh, 255, cv2.THRESH_BINARY)

    # 二值化后的图像
    # cv2.imshow("1",img_bin)
    # cv2.waitKey(0)
    # 二值化
    robotpixel = int(robotsize / ratio)
    erode_r = int(robotpixel / 2)
    element = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (erode_r, erode_r))
    # 构造核
    erosion = cv2.erode(img_bin, element, iterations=1)  # 腐蚀
    # erosion是腐蚀后的图像
    # cv2.imshow("1",erosion)
    # cv2.waitKey(0)
    return [get_approx(erosion, robotpixel), list(img_bin.shape)]


def find_concave(point_map):
    s_point_temp = 0
    n_point_temp = 0
    point_list_len = 0
    point_list = []
    linelist = []
    linelistlen = 0
    concave_list = []
    concave_listlen = 0

    while True:
        point_list_len += 1

        while True:
            if n_point_temp + 2 >= len(point_map) or point_map[n_point_temp + 2][0] < 0:
                #遍历到倒数第二个点，实际上在判断最后一个点
                linelist.append([n_point_temp, n_point_temp + 1])
                v1_x = point_map[n_point_temp + 1][0] - point_map[n_point_temp][0]
                v1_y = point_map[n_point_temp + 1][1] - point_map[n_point_temp][1]
                v2_x = point_map[s_point_temp][0] - point_map[n_point_temp + 1][0]
                v2_y = point_map[s_point_temp][1] - point_map[n_point_temp + 1][1]
                r = v1_x * v2_y - v1_y * v2_x

                if r > 0:
                    point_list.append([n_point_temp, n_point_temp + 1, s_point_temp, 1])
                else:
                    point_list.append([n_point_temp, n_point_temp + 1, s_point_temp, -1])
                    concave_list.append([n_point_temp, n_point_temp + 1, s_point_temp, -1])

                #遍历到倒数第一个点，实际上在判断起点
                n_point_temp += 1
                linelist.append([n_point_temp, s_point_temp])

                v1_x = point_map[s_point_temp][0] - point_map[n_point_temp][0]
                v1_y = point_map[s_point_temp][1] - point_map[n_point_temp][1]
                v2_x = point_map[s_point_temp + 1][0] - point_map[s_point_temp][0]
                v2_y = point_map[s_point_temp + 1][1] - point_map[s_point_temp][1]
                r = v1_x * v2_y - v1_y * v2_x

                if r > 0:
                    point_list.append( [n_point_temp, s_point_temp, s_point_temp + 1, 1])
                else:
                    point_list.append([n_point_temp, s_point_temp, s_point_temp + 1, -1])
                    concave_list.append([n_point_temp, s_point_temp, s_point_temp + 1, -1])

                point_list_len += 1
                break

            else:
                linelist.append([n_point_temp, n_point_temp + 1])
                v1_x = point_map[n_point_temp + 1][0] - point_map[n_point_temp][0]
                v1_y = point_map[n_point_temp + 1][1] - point_map[n_point_temp][1]
                v2_x = point_map[n_point_temp + 2][0] - point_map[n_point_temp + 1][0]
                v2_y = point_map[n_point_temp + 2][1] - point_map[n_point_temp + 1][1]
                r = v1_x * v2_y - v1_y * v2_x

                if r > 0:
                    point_list.append([n_point_temp, n_point_temp + 1, n_point_temp + 2, 1])
                else:
                    point_list.append([n_point_temp, n_point_temp + 1, n_point_temp + 2, -1])
                    concave_list.append([n_point_temp, n_point_temp + 1, n_point_temp + 2, -1])

            n_point_temp += 1
        n_point_temp += 1
        if n_point_temp >= len(point_map):
            break
        else:
            n_point_temp += 1
            s_point_temp = n_point_temp

    # 返回计算结果
    return point_list, linelist, concave_list


if __name__ == "__main__":

    path = '/home/sy/amrrt/demos/resources/output9.png'
    ratio = 0.02
    robotsize = 0.6
    thresh = 128

    IMGmap = cv2.imread(path)
    #cv2.imshow("1", IMGmap)
    returndata = approx_work(IMGmap, ratio, robotsize, thresh)
    # 输入处理过后的路径，比例，机器人大小，
    for i in range(0,len(returndata[0])):#遍历连通域
        find_concave(returndata[0][i])
    # temp = np.ones((1463, 1218,3), np.uint8) * 255
    # cv2.drawContours(temp, approxs, -1, (0, 255, 0), 1)
    # for i in range(0, len(returnlist)):
    #     color = (random.randint(0, 255), random.randint(0, 255), random.randint(0, 255))
    #     #print(returnlist[i])
    #     for point in returnlist[i] :
    #          print(point)
    #          if point[0]!=-1:
    #             cv2.circle(temp, point, 5, color, 2)
    #          else:
    #             color = (random.randint(0, 255), random.randint(0, 255), random.randint(0, 255))
    # cv2.imshow('approxs', temp)
    # cv2.waitKey()
    #find_concave(returndata[0])




